﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using OpenNI;

namespace BTKinect.DataType
{
    class KinectVector : Vector
    {
        #region CONSTRUCTORS
        public KinectVector() : base(3) { }
        public KinectVector(double[] components) : base(components) { }
        public KinectVector(int n) : base(n) { }
        public KinectVector(int n, double value) : base(n, value) { }
        public KinectVector(int n, int i) : base(n, i) { }
        public KinectVector(Vector v): base(3)
        {
            KinectVector k = this;
            k[0] = v[0];
            k[1] = v[1];
            k[2] = v[2];
        }
        #endregion

        #region CONVERSIONS
        public static explicit operator Point3D(KinectVector v)
        {
            Point3D p = new Point3D();
            p.X = (float)v[0];
            p.Y = (float)v[1];
            p.Z = (float)v[2];
            return p;
        }
        public static explicit operator KinectPoint3D(KinectVector v)
        {
            return new KinectPoint3D(v[0], v[1], v[2]);
        }
        public static explicit operator double[](KinectVector v)
        {
            int l = v.Length;
            double[] d = new double[l];
            for (int i = 0; i < l; i++)
            {
                d[i] = v[0];
            }
            return d;
        }
        public static implicit operator KinectVector(double[] d)
        {
            KinectVector kv = new KinectVector();
            kv[0] = d[0];
            kv[1] = d[1];
            kv[2] = d[2];
            return kv;
        }
        #endregion

        #region OPERATORS
        public static KinectVector operator -(KinectVector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] - v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator +(KinectVector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] + v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator *(KinectVector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] * v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator /(KinectVector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] / v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }

        public static KinectVector operator -(KinectVector v1, float f)
        {
            int l1 = v1.Length;
            double[] d = new double[3];
            for (int i = 0; i < l1; i++)
            {
                d[i] = v1[i] - f;
            }
            return new KinectVector(d);
        }
        public static KinectVector operator +(KinectVector v1, float f)
        {
            int l1 = v1.Length;
            double[] d = new double[3];
            for (int i = 0; i < l1; i++)
            {
                d[i] = v1[i] + f;
            }
            return new KinectVector(d);
        }
        public static KinectVector operator *(KinectVector v1, float f)
        {
            int l1 = v1.Length;
            double[] d = new double[3];
            for (int i = 0; i < l1; i++)
            {
                d[i] = v1[i] * f;
            }
            return new KinectVector(d);
        }
        public static KinectVector operator /(KinectVector v1, float f)
        {
            int l1 = v1.Length;
            double[] d = new double[3];
            for (int i = 0; i < l1; i++)
            {
                d[i] = v1[i] / f;
            }
            return new KinectVector(d);
        }

        public static KinectVector operator -(KinectVector v1, Vector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] - v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator +(KinectVector v1, Vector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] + v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator *(KinectVector v1, Vector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] * v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator /(KinectVector v1, Vector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] / v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }

        public static KinectVector operator -(Vector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] - v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator +(Vector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] + v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator *(Vector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] * v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        public static KinectVector operator /(Vector v1, KinectVector v2)
        {
            int l1 = v1.Length;
            int l2 = v2.Length;
            double[] d = new double[3];
            if (l1 == l2)
            {
                for (int i = 0; i < l1; i++)
                {
                    d[i] = v1[i] / v2[i];
                }
                return new KinectVector(d);
            }
            else
                throw new ArgumentException("Vectors must have same length to be subtracted");
        }
        #endregion
    }
}
